﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace DARE
{
    public class CNodePath
    {

        #region fields

        private const float m_cWeight = 0.6f;

        private CRender3D m_render;
        private List<CNode> m_nodes;
        private List<CCurve> m_curves;
        private List<CCurveModel> m_models;
        private List<float> m_weights;

        private bool m_needUpdateLength;
        private float m_length;
        private bool m_show;
        private bool m_useRoll;

        private CNode m_node;

        #endregion

        #region properties

        public float Length
        {
            get
            {
                if (m_needUpdateLength)
                    ComputeLength();
                return m_length;
            }
        }

        public int Count
        {
            get { return m_nodes.Count; }
        }

        /// <summary>
        /// Use at your own risks.
        /// Changing a node by this property may have unwanted effects.
        /// </summary>
        public List<CNode> Nodes
        {
            get { return m_nodes; }
        }

        public List<float> Weights
        {
            get { return m_weights; }
        }

        public bool Show
        {
            get { return m_show; }
            set
            {
                if (value == m_show)
                    return;
                m_show = value;
                foreach (CNode node in m_nodes)
                    node.Entity.Model.Show = m_show;
            }
        }

        public bool UseRoll
        {
            get { return m_useRoll; }
            set { m_useRoll = value; }
        }

        #endregion

        #region ctor

        public CNodePath(CRender3D render)
        {
            Initialize(render);
        }

        private void Initialize(CRender3D render)
        {
            m_render = render;
            m_nodes = new List<CNode>();
            m_curves = new List<CCurve>();
            m_models = new List<CCurveModel>();
            m_weights = new List<float>();
            m_needUpdateLength = true;
            m_show = false;
            m_node = new CNode(EType.DYNAMIC, false);
            m_useRoll = false;
        }

        #endregion

        #region add

        public void AddNode()
        {
            AddNode(Vector3.Zero, Quaternion.Identity, m_cWeight);
        }

        public void AddNode(Vector3 position)
        {
            AddNode(position, Quaternion.Identity, m_cWeight);
        }

        public void AddNode(float x, float y, float z, float yaw, float pitch, float roll)
        {
            AddNode(x, y, z, yaw, pitch, roll, m_cWeight);
        }

        public void AddNode(float x, float y, float z, float yaw, float pitch, float roll, float weight)
        {
            AddNode(new Vector3(x, y, z), Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll), weight);
        }

        public void AddNode(Vector3 position, Quaternion orientation)
        {
            AddNode(position, orientation, m_cWeight);
        }

        public void AddNode(Vector3 position, Quaternion orientation, float weight)
        {
            CNode node =m_render.CreateNode(EType.STATIC, true).SetPosition(position, CNode.ESpace.WORLD).
                SetOrientation(orientation, CNode.ESpace.WORLD);

            node.AABB = new BoundingBox(new Vector3(-50, -50, -50), new Vector3(50, 50, 50));
            m_nodes.Add(node);
            m_curves.Add(new CCurve());
            m_models.Add(new CCurveModel());
            m_weights.Add(m_cWeight);
            SetNode(m_nodes.Count - 1, position, orientation, weight);
        }

        #endregion

        #region insert

        public void InsertNode(int index)
        {
            InsertNode(index, Vector3.Zero, Quaternion.Identity, m_cWeight);
        }

        public void InsertNode(int index, Vector3 position)
        {
            InsertNode(index, position, Quaternion.Identity, m_cWeight);
        }

        public void InsertNode(int index, float x, float y, float z, float yaw, float pitch, float roll)
        {
            InsertNode(index, x, y, z, yaw, pitch, roll, m_cWeight);
        }

        public void InsertNode(int index, float x, float y, float z, float yaw, float pitch, float roll, float weight)
        {
            InsertNode(index, new Vector3(x, y, z), Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll), weight);
        }

        public void InsertNode(int index, Vector3 position, Quaternion orientation)
        {
            InsertNode(index, position, orientation, m_cWeight);
        }

        public void InsertNode(int index, Vector3 position, Quaternion orientation, float weight)
        {
            CNode node = m_render.CreateNode(EType.STATIC, true).SetPosition(position, CNode.ESpace.WORLD)
                .SetOrientation(orientation, CNode.ESpace.WORLD);

            node.AABB = new BoundingBox(new Vector3(-50, -50, -50), new Vector3(50, 50, 50));
            m_nodes.Insert(index, node);
            m_curves.Insert(index, new CCurve());
            m_models.Insert(index, new CCurveModel());
            m_weights.Insert(index, m_cWeight);
            SetNode(index, position, orientation, weight);
        }

        #endregion

        #region set

        public void SetNode(int index)
        {
            SetNode(index, Vector3.Zero, Quaternion.Identity, m_cWeight);
        }

        public void SetNode(int index, Vector3 position)
        {
            SetNode(index, position, Quaternion.Identity, m_cWeight);
        }

        public void SetNode(int index, float x, float y, float z, float yaw, float pitch, float roll)
        {
            SetNode(index, x, y, z, yaw, pitch, roll, m_cWeight);
        }

        public void SetNode(int index, float x, float y, float z, float yaw, float pitch, float roll, float weight)
        {
            SetNode(index, new Vector3(x, y, z), Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll), weight);
        }

        public void SetNode(int index, Vector3 position, Quaternion orientation)
        {
            SetNode(index, position, orientation, m_cWeight);
        }

        public void SetNode(int index, Vector3 position, Quaternion orientation, float weight)
        {
            // we have a list so the previous index when == 0 is the end of the list, inverse for the next
            int prev = index <= 0 ? m_nodes.Count - 1 : index - 1;
            int next = index >= m_nodes.Count - 1 ? 0 : index + 1;
            float distance = 0;
            Quaternion conjugate = Quaternion.Identity;


            // node position
            m_nodes[index].Position = position;
            m_nodes[index].Orientation = orientation;
            m_weights[index] = weight;


            // end of the previous point
            // distance from the previous point to the current point
            distance = Vector3.Distance(m_nodes[prev].Position, m_nodes[index].Position);
            m_curves[prev].End = position;
            m_curves[prev].CtrlB = position + Vector3.Transform(Vector3.UnitZ * distance * m_weights[index], 
                m_nodes[index].Orientation);
            // needed when adding nodes not in croissant order
            m_curves[prev].Start = m_nodes[prev].Position;
            m_curves[prev].CtrlA = m_nodes[prev].Position + Vector3.Transform(-Vector3.UnitZ * distance * m_weights[prev],
                m_nodes[prev].Orientation);

            // start of the current point
            // distance from the current point to the next point
            distance = Vector3.Distance(m_nodes[index].Position, m_nodes[next].Position);
            m_curves[index].Start = position;
            m_curves[index].CtrlA = position + Vector3.Transform(-Vector3.UnitZ * distance * m_weights[index], 
                m_nodes[index].Orientation);
            // needed when adding nodes not in croissant order
            m_curves[index].End = m_nodes[next].Position;
            m_curves[index].CtrlB = m_nodes[next].Position + Vector3.Transform(Vector3.UnitZ * distance * m_weights[next], 
                m_nodes[next].Orientation);


            // previous curve model (the curve is in an absolute position so we need to offset the model by - node.Position)
            // same principle for the orientation
            conjugate = m_nodes[prev].Orientation;
            conjugate.Conjugate();
            m_models[prev] = new CCurveModel(m_curves[prev], 35);
            m_models[prev].Offset -= m_nodes[prev].Position;
            m_models[prev].Orientation = Matrix.CreateFromQuaternion(conjugate);
            // current curve model
            conjugate = m_nodes[index].Orientation;
            conjugate.Conjugate();
            m_models[index] = new CCurveModel(m_curves[index], 35);
            m_models[index].Offset -= m_nodes[index].Position;
            m_models[index].Orientation = Matrix.CreateFromQuaternion(conjugate);


            // attach models
            m_nodes[prev].Entity.AttachModel(m_models[prev]);
            m_nodes[index].Entity.AttachModel(m_models[index]);


            // set to update the length since the nodepath changed
            m_needUpdateLength = true;
        }

        #endregion

        #region remove

        public void Clear()
        {
            foreach (CNode node in m_nodes)
                m_render.RemoveNode(node);
            m_nodes.Clear();
            m_curves.Clear();
            m_models.Clear();
            m_weights.Clear();
        }

        public void RemoveNode(int index)
        {
            m_render.RemoveNode(m_nodes[index]);
            m_nodes.RemoveAt(index);
            m_curves.RemoveAt(index);
            m_models.RemoveAt(index);
            m_weights.RemoveAt(index);
            UpdateNode(index);
        }

        #endregion

        #region update

        private void UpdateNode(int index)
        {
            int prev = index <= 0 ? m_nodes.Count - 1 : index - 1;
            int next = index >= m_nodes.Count - 1 ? 0 : index + 1;

            SetNode(prev, m_nodes[prev].Position, m_nodes[prev].Orientation, m_weights[prev]);
            SetNode(next, m_nodes[next].Position, m_nodes[next].Orientation, m_weights[next]);
        }

        #endregion

        #region methods

        private void ComputeLength()
        {
            m_length = 0;
            foreach (CCurve curve in m_curves)
                m_length += curve.Length;
            m_needUpdateLength = false;
        }

        //public Vector3 GetPosDistance(float dist, out Vector3 tangent)
        //{
        //    float currentLength = 0;
        //    float lastLength = 0;

        //    foreach (CCurve curve in m_curves)
        //    {
        //        lastLength = currentLength;
        //        currentLength += curve.Length;
        //        if (dist >= lastLength && dist <= currentLength)
        //        {
        //            tangent = curve.GetTangent((dist - lastLength) / curve.Length);
        //            return curve.GetPos((dist - lastLength) / curve.Length);
        //        }
        //    }
        //    tangent = Vector3.Zero;
        //    return Vector3.Zero;
        //}

        public Vector3 GetPosDistance(float dist, out Quaternion orientation)
        {
            float currentLength = 0;
            float lastLength = 0;
            int max = m_curves.Count;
            float currentDist = 0;
            float precision = 0.1f;
            CCurve curve = null;
            Vector3 pos = Vector3.Zero;
            Vector3 tangent = Vector3.Zero;

            orientation = Quaternion.Identity;
            for (int i = 0; i < max; ++i)
            {
                curve = m_curves[i];
                lastLength = currentLength;
                currentLength += curve.Length;
                if (dist >= lastLength && dist <= currentLength)
                {
                    // ======== position ==================

                    currentDist = dist - lastLength;
                    pos = curve.GetPos(currentDist / curve.Length);

                    // ======== tangent approximation ========

                    if (currentDist + precision <= curve.Length)
                        tangent = curve.GetPos((currentDist + precision) / curve.Length) - pos;
                    tangent.Normalize();

                    // ======== orientation ================

                    m_node.LookAt(tangent);

                    orientation = m_node.Orientation;

                    if (m_useRoll)
                    {
                        int next = i == m_nodes.Count - 1 ? 0 : i + 1;
                        float roll = (float)MathHelper.Lerp(m_nodes[i].NRoll, m_nodes[next].NRoll, currentDist / curve.Length);
                        //roll += MathHelper.TwoPi;
                        orientation *= Quaternion.CreateFromYawPitchRoll(0, 0, roll);
                        //orientation *= Quaternion.CreateFromYawPitchRoll(0, 0, 0.2f);
                    }

                    // =================================

                    break;
                }
            }
            return pos;
        }

        //public Vector3 GetPosDistance(float dist, out Vector3 tangent)
        //{
        //    float currentLength = 0;
        //    float lastLength = 0;
        //    int max = m_curves.Count;
        //    float currentDist = 0;
        //    float precision = 0.1f;
        //    CCurve curve = null;
        //    Vector3 pos = Vector3.Zero;

        //    tangent = Vector3.Zero;
        //    for (int i = 0; i < max; ++i)
        //    {
        //        curve = m_curves[i];
        //        lastLength = currentLength;
        //        currentLength += curve.Length;
        //        if (dist >= lastLength && dist <= currentLength)
        //        {
        //            currentDist = dist - lastLength;
        //            pos = curve.GetPos(currentDist / curve.Length);

        //            // ======== tangent approximation ========

        //            //tangent = curve.GetTangent((dist - lastLength) / curve.Length);

        //            if (currentDist + precision <= curve.Length)
        //                tangent = curve.GetPos((currentDist + precision) / curve.Length) - pos;
        //            // ======================
        //            break;
        //        }
        //    }
        //    return pos;
        //}

        #endregion

    }
}
